package nezumi.state;

import java.awt.Point;
import java.awt.Polygon;
import java.awt.geom.Area;
import java.awt.geom.Point2D;
import java.io.IOException;
import java.util.Observable;
import java.util.Observer;
import java.util.logging.Logger;

import javax.xml.parsers.DocumentBuilder;
import javax.xml.parsers.DocumentBuilderFactory;
import javax.xml.parsers.ParserConfigurationException;

import nezumi.Constants;
import nezumi.sensing.IRDiscreteSensorModel;
import nezumi.sensing.ISensorModel;
import nezumi.util.AngleGeometry;
import nezumi.util.XMLUtils;

import org.w3c.dom.Document;
import org.w3c.dom.Node;
import org.w3c.dom.NodeList;
import org.xml.sax.SAXException;

public class MapState implements Observer {

	public static double WALKABLE_THRESHOLD = Constants.WALKABLE_THRESHOLD;
	private static final float VISITED_RATIO = Constants.VISITED_GRANULARITY
			/ Constants.MAP_GRANULARITY;
	private static final ISensorModel irSensorModel = new IRDiscreteSensorModel();
	private static final Logger logger = Logger.getLogger(MapState.class
			.getName());
	private static final float GRID_INIT_VAL = 0.5f;
	private static final double[] sensorRelativeAngles = {
			Math.toRadians(Constants.IR_FRT_ANGLE),
			Math.toRadians(Constants.IR_LFT_ANGLE),
			Math.toRadians(Constants.IR_RGT_ANGLE) };
	private static final double irHalfAperture = Math
			.toRadians(Constants.IR_APERTURE) / 2;

	// we store the maze as a grid
	// in each position we have the probability p(H)
	// of that cell having a wall/obstacle
	// by default, this probability is zero, corresponding
	// to an empty maze
	private double occupancyGrid[][];
	private int visitedGrid[][];
	private final int width, height;
	private State state;
	private final Point2D deltaPoint = new Point2D.Double(0, 0);

	public MapState(State state, int width, int height) {
		assert state != null : "state = " + state;
		assert width > 0 : "width = " + width;
		assert height > 0 : "height = " + height;

		this.state = state;
		this.width = width;
		this.height = height;

		occupancyGrid = new double[width][height];

		visitedGrid = new int[(int) Math.ceil(Constants.MAP_WIDTH
				/ Constants.VISITED_GRANULARITY * 2)][(int) Math
				.ceil(Constants.MAP_HEIGHT / Constants.VISITED_GRANULARITY * 2)];

		clearOccupancyGrid();
		clearOccupancyGridAroundPos(this.state.getAgentState().getCurrPos());
	}

	/**
	 * Set all cells to "unknown".
	 */
	public void clearOccupancyGrid() {
		// initialize to -1.0 meaning unknown
		for (int i = 0; i < width; i++)
			for (int j = 0; j < height; j++)
				occupancyGrid[i][j] = GRID_INIT_VAL;
	}

	public void clearOccupancyGridAroundPos(Point2D pos) {

		final float griddelta = Constants.MOUSE_RADIUS
				/ Constants.MAP_GRANULARITY;
		int minx = (int) Math.round(pos.getX() - griddelta), maxx = (int) Math
				.round(pos.getX() + griddelta), miny = (int) Math.round(pos
				.getY()
				- griddelta), maxy = (int) Math.round(pos.getY() + griddelta);
		minx = minx > 0 ? minx : 0;
		maxx = maxx < this.width ? maxx : this.width - 1;
		miny = miny > 0 ? miny : 0;
		maxy = maxy < this.height ? maxy : this.height - 1;

		for (int i = minx; i <= maxx; i++)
			for (int j = miny; j <= maxy; j++) {
				final double deltax = pos.getX() - i, deltay = pos.getY() - j;
				if (Math.sqrt(deltax * deltax + deltay * deltay)
						* Constants.MAP_GRANULARITY < Constants.MOUSE_RADIUS)
					occupancyGrid[i][j] = 0;
			}
	}

	/**
	 * Called to update the occupancy grid when the obstacle sensors detect
	 * something.
	 */
	public void updateOccupancyGrid() {

		// compute bounding box
		final AgentState astate = state.getAgentState();
		final Point2D currPos = astate.getCurrPos();
		final double currOrientation = astate.getCurrOrientation();
		final float griddelta = (2 * Constants.IR_CUTOFF)
				/ Constants.MAP_GRANULARITY;
		int minx = (int) Math.round(currPos.getX() - griddelta), maxx = (int) Math
				.round(currPos.getX() + griddelta), miny = (int) Math
				.round(currPos.getY() - griddelta), maxy = (int) Math
				.round(currPos.getY() + griddelta);
		minx = minx > 0 ? minx : 0;
		maxx = maxx < this.width ? maxx : this.width - 1;
		miny = miny > 0 ? miny : 0;
		maxy = maxy < this.height ? maxy : this.height - 1;

		// Sensor values for front, left and right sensors
		SensorState sstate = state.getSensorState();
		final double[] sensorValues = { sstate.getSensor(SensorState.IR_FRT),
				sstate.getSensor(SensorState.IR_LFT),
				sstate.getSensor(SensorState.IR_RGT) };

		final double[] sensorAngles = new double[3];
		for (int i = 0; i < 3; i++)
			sensorAngles[i] = currOrientation + sensorRelativeAngles[i];

		// walk through every cell and update the occupancy probability
		// using the bayes update rule
		for (int i = minx; i <= maxx; i++)
			for (int j = miny; j <= maxy; j++) {
				for (int k = 0; k < sensorValues.length; k++) {

					if (sensorValues[k] >= Constants.IR_CUTOFF
							&& Math.abs(AngleGeometry.angleDifference(
									sensorAngles[k], Math.atan2(j
											- currPos.getY(), i
											- currPos.getX()))) < irHalfAperture) {

						final double deltax = currPos.getX() - i, deltay = currPos
								.getY()
								- j;
						final double dist = (Math.sqrt(deltax * deltax + deltay
								* deltay) * Constants.MAP_GRANULARITY)
								- Constants.MOUSE_RADIUS;

						// compute the probability according to the active
						// sensor model
						final double prob = irSensorModel.computeProb(
								sensorValues[k], dist);

						
						//unknown  area
						if(prob == -1.0)
							continue;
						
						assert prob >= 0 && prob <= 1 : "prob = " + prob;

						/*
						 * The Bayesian update rule being: P(H|Sn) =
						 * P(Sn|H)*P(H|Sn-1) / P(Sn|H)*P(H|Sn-1) +
						 * P(Sn|~H)*P(~H|Sn-1)
						 */
						// final double old = occupancyGrid[i][j];
						occupancyGrid[i][j] = (prob * occupancyGrid[i][j])
								/ ((prob * occupancyGrid[i][j]) + ((1 - prob) * (1 - occupancyGrid[i][j])));

						// System.out .printf( "grid[%s][%s], old=%.2f,
						// dist=%.2f, 1/dist=%.2f reading=%.2f, prob=%.2f,
						// new=%.2f\n", i, j, old,
						// dist, 1/dist, sensorValues[k], prob,
						// occupancyGrid[i][j]);

						// make sure it is
						assert occupancyGrid[i][j] >= 0
								&& occupancyGrid[i][j] <= 1 : String.format(
								"grid[%d][%d]=%.2f", i, j, occupancyGrid[i][j]);

						// System.out.printf("grid[%s][%s]=%.2f\n", i, j,
						// occupancyGrid[i][j]);
					}
				}
			}

		// let us plan once more
		this.state.getPlanner().plan();
	}

	/**
	 * Loads a complete map directly from the XML specification files.
	 */
	public void loadMapState(final String labfile, final String gridfile)
			throws SAXException, IOException {

		assert labfile != null : "labfile = " + labfile;
		assert gridfile != null : "gridfile = " + gridfile;

		// factory design pattern
		DocumentBuilderFactory fac = DocumentBuilderFactory.newInstance();
		DocumentBuilder db = null;
		try {
			db = fac.newDocumentBuilder();
		} catch (ParserConfigurationException e) {
			logger
					.severe(String
							.format(
									"Error creating %s for loading a map from an XML labyrinth file. Exception: %s",
									db.getClass().getName(), e.getMessage()));
			return;
		}

		// parse the grid file
		Document doc = db.parse(gridfile);

		// we are only interested in the first target for now
		final Node startNode = doc.getElementsByTagName("Position").item(0);

		final Point2D startPos = this.state.getAgentState().getStartPos();
		double deltax = (startPos.getX() / (0.1 / Constants.MAP_GRANULARITY))
				- (Double.parseDouble(startNode.getAttributes().getNamedItem(
						"X").getTextContent()) * 10);
		double deltay = (startPos.getY() / (0.1 / Constants.MAP_GRANULARITY))
				- (Double.parseDouble(startNode.getAttributes().getNamedItem(
						"Y").getTextContent()) * 10);

		this.deltaPoint
				.setLocation(
						startPos.getX()
								- (Double.parseDouble(startNode.getAttributes()
										.getNamedItem("X").getTextContent()) / Constants.MAP_GRANULARITY),
						startPos.getY()
								- (Double.parseDouble(startNode.getAttributes()
										.getNamedItem("Y").getTextContent()) / Constants.MAP_GRANULARITY));

		// parse the labyrinth file
		doc = db.parse(labfile);

		// process the walls: the idea is to start with an empty
		// area and then add to it as we read more and more
		// walls. In the end, intersect with each of the map cells
		// to decide where the obstacles are.
		// create an augmented wall that takes the mouse's size into account
		final Area wallArea = new Area();
		final NodeList wallList = doc.getElementsByTagName("Wall");
		for (int i = 0; i < wallList.getLength(); i++) {
			final NodeList cornerList = wallList.item(i).getChildNodes();

			double x, y;
			Polygon wall = new Polygon();
			for (int j = 0; j < cornerList.getLength(); j++) {

				final Node cornerNode = cornerList.item(j);
				// guard against unexpected nodes
				if (!cornerNode.getNodeName().toLowerCase().equals("corner"))
					continue;

				x = XMLUtils.readDoubleAttribute(cornerNode, "X") * 10;
				y = XMLUtils.readDoubleAttribute(cornerNode, "Y") * 10;
				wall.addPoint((int) (x + deltax), (int) (y + deltay));
			}

			// add to the total area
			wallArea.add(new Area(wall));
		}

		final double width2 = width / (0.1 / Constants.MAP_GRANULARITY);
		final double height2 = height / (0.1 / Constants.MAP_GRANULARITY);

		// let us add the external bounding box that isn't explicitly
		// defined in the map file
		final Polygon bound = new Polygon();
		// bottom
		bound.addPoint((int) deltax, (int) deltay);

		bound.addPoint((int) (width2 / 2 + deltax), (int) deltay + 1);
		bound.addPoint((int) (width2 / 2 + deltax), (int) deltay);
		bound.addPoint((int) deltax + 1, (int) deltay);
		wallArea.add(new Area(bound));
		bound.reset();
		// left
		bound.addPoint((int) deltax, (int) deltay);
		bound.addPoint((int) deltax, (int) (height2 / 2 + deltay));
		bound.addPoint((int) deltax + 1, (int) (height2 / 2 + deltay));
		bound.addPoint((int) deltax, (int) deltay + 1);
		wallArea.add(new Area(bound));
		bound.reset();
		// top
		bound.addPoint((int) deltax, (int) (height2 / 2 + deltay));
		bound.addPoint((int) (width2 / 2 + deltax),
				(int) (height2 / 2 + deltay));
		bound.addPoint((int) (width2 / 2 + deltax),
				(int) (height2 / 2 + deltay + 1));
		bound.addPoint((int) deltax, (int) (height2 / 2 + deltay + 1));
		wallArea.add(new Area(bound));
		bound.reset();
		// right
		bound.addPoint((int) (width2 / 2 + deltax),
				(int) (height2 / 2 + deltay));
		bound.addPoint((int) (width2 / 2 + deltax), (int) deltay);
		bound.addPoint((int) (width2 / 2 + 1 + deltax), (int) deltay);
		bound.addPoint((int) (width2 / 2 + 1 + deltax),
				(int) (height2 / 2 + deltay));
		wallArea.add(new Area(bound));
		bound.reset();
		// now that we have the wall area completely defined, let us intersect
		// with the map cells
		double[][] gridcopy = new double[this.width][this.height];
		for (int i = 0; i < this.width; i++)
			for (int j = 0; j < this.height; j++) {
				// 100% chance of obstacle if inside the wall area, 0% otherwise

				occupancyGrid[i][j] = wallArea.intersects(i
						/ (0.1 / Constants.MAP_GRANULARITY), j
						/ (0.1 / Constants.MAP_GRANULARITY),
						Constants.MAP_GRANULARITY * 10,
						Constants.MAP_GRANULARITY * 10) ? 1.0f : 0.0f;
				gridcopy[i][j] = occupancyGrid[i][j];
			}

		// now we have walls so we need to recompute the path
		this.state.getPlanner().plan();
	}

	/**
	 * Returns true if there is a good chance the grid position represented by
	 * "point" doesn't have an obstacle in it.
	 * 
	 * @param point
	 * @return
	 */
	public boolean isWalkable(Point point) {
		assert point != null : "point = " + point;

		return isWalkable(point.x, point.y);
	}

	/**
	 * Returns true if there is a good chance the grid position (x,y) doesn't
	 * have an obstacle in it.
	 * 
	 * @param x
	 * @param y
	 * @return
	 */
	public boolean isWalkable(int x, int y) {
		assert x >= 0 && x < width : "x = " + x;
		assert x >= 0 && x < width : "y = " + y;

		final float griddelta = Constants.MOUSE_RADIUS
				/ Constants.MAP_GRANULARITY;
		int minx = (int) (x - griddelta), maxx = (int) (x + griddelta), miny = (int) (y - griddelta), maxy = (int) (y + griddelta);
		minx = minx > 0 ? minx : 0;
		maxx = maxx < this.width ? maxx : this.width - 1;
		miny = miny > 0 ? miny : 0;
		maxy = maxy < this.height ? maxy : this.height - 1;

		for (int i = minx; i <= maxx; i++)
			for (int j = miny; j <= maxy; j++) {
				if (occupancyGrid[i][j] > WALKABLE_THRESHOLD)
					return false;
			}

		return true;
	}

	public int getWidth() {
		return width;
	}

	public int getHeight() {
		return height;
	}

	public double[][] getOccupancyGrid() {
		return occupancyGrid;
	}

	/**
	 * We should be notified every time new sensor values come along.
	 */
	public void update(Observable o, Object arg) {
		if (arg instanceof String
				&& SensorState.IR_ALL.equalsIgnoreCase((String) arg))
			this.updateOccupancyGrid();
	}

	public Point2D scalePoint(final Point2D p) {

		return new Point2D.Double((p.getX() - this.deltaPoint.getX())
				* Constants.MAP_GRANULARITY,
				(p.getY() - this.deltaPoint.getY()) * Constants.MAP_GRANULARITY);
	}

	private int toVisitedGridCoords(double x) {
		return (int) (x / VISITED_RATIO);
	}

	public void updateVisitedGrid(final Point2D newPos) {

		++visitedGrid[toVisitedGridCoords(newPos.getX())][toVisitedGridCoords(newPos
				.getY())];
	}
}